package endcourseproject;

import java.util.ArrayList;
import java.util.Iterator;

import lejos.nxt.LCD;


public class RobotManager implements ZigBeeListener 
{
	
    // list of robot info about fellow flock robots
	private ArrayList<RobotInfo> _robots = new ArrayList<RobotInfo>();
	private ZigBeeDevice _device = null;
	private int txCount = 0;

	// status (msg) of this robot, the status is periodically sent to the fellow robots in the flock
	private RobotStatusMsg robotStatus = null;
	
	public RobotStatusMsg getRobotStatus() {
		return robotStatus;
	}

	public void setRobotStatus(RobotStatusMsg robotStatus) {
		this.robotStatus = robotStatus;
	}

	public int getTxCount() {
		return txCount;
	}

	public int getRxCount() {
		return rxCount;
	}
	private int rxCount = 0;
	
	public ZigBeeDevice getDevice() {
		return _device;
	}
	
	public RobotManager(ZigBeeDevice zbd)
	{
		_device = zbd;
		_device.setListener(this);

		robotStatus = new RobotStatusMsg(); // default initialized status msg
	}
	
	public RobotInfo getFlockRobotInfo(int index)
	{
		RobotInfo rInf;
		synchronized(_robots)
 		{
			////LCD.drawString("lock 3 start", 0, 3);
	 		rInf = _robots.get(index);
			////LCD.drawString("lock 3 end  ", 0, 3);
 		}
 		return rInf;
	}
	public int getNumFlockRobotInfo()
	{
		int size;
		synchronized(_robots)
 		{
			////LCD.drawString("lock 4 start", 0, 3);
	 		size = _robots.size();
			////LCD.drawString("lock 4 end  ", 0, 3);
 		}
		return size;
	}

	public void sendMessage()
	{
		if (robotStatus!= null)
		{
			_device.BroadcastDataRequest(robotStatus.getRawMsg());
		}
		else
		{
			_device.BroadcastDataRequest(new byte[] {  0x00, 0x01, 0x02, 0x03, 0x04 });
		}	
		//  _device.BroadcastDataRequest(new byte[] {  0x00, 0x01, 0x02, 0x03, 0x04 });
		txCount++;
	}
	@Override
	public void message(MCPS_dataIndication message)
	{
		byte[] addr = message.getSrcAddr();
		int signal = (int)message.getMpduLinkQuality() & 0xFF;
		boolean found = false;
		rxCount++;
		synchronized (_robots) 
		{
	 		////LCD.drawString("lock 5", 0, 3);
			// Lock access
			RobotInfo ri = null;
			
			int size = _robots.size();
			for( int i = 0 ; i < size && !found; i++ )
			{
				ri = _robots.get(i);
				found = ri.equals(addr);
			}
			
			if( found )
			{
				ri.setSignalStrength(signal);
			}
			else
			{
				ri = new RobotInfo();
				ri.setAddress(addr);
				ri.setSignalStrength(signal);
				_robots.add(ri);
			}	

			if (message.getMsduLength()>=1)
			{
				RobotMsg rm = new RobotMsg(message.getMsdu());
				if (rm.getMsgType() == RobotMsg.StatusMsgType && message.getMsdu().length == RobotStatusMsg.MsgLen)
				{
                    ri.setStatusMsg(new RobotStatusMsg(rm));					
				}

				if (rm.getMsgType() == RobotMsg.TravelCmdMsgType && message.getMsdu().length == RobotTravelCmdMsg.MsgLen)
				{
					RobotTravelCmdMsg tcmd = new RobotTravelCmdMsg(rm);
					// TODO what to do with this info
				
				}
			}	
		}
	}

}
